﻿using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using System;
using System.Collections.Generic;


namespace cubeAdjustment
{
    //TODO:优化程序
    internal class PlaneFeature
    {
        internal double sigma0;
        internal List<AngleObs> angleobs;
        internal List<DistanceObs> distanceobs;
        internal Matrix<double> B, P;
        internal Matrix<double> N;
        internal Vector<double> W, X, V;
        internal Vector<double> l;
        public PlaneFeature(GetCoordinate coordinate, List<iObserve> observedDatas, knownData knownData)
        {
            angleobs = new List<AngleObs>();
            distanceobs = new List<DistanceObs>();
            foreach (iObserve observedData in observedDatas)
            {
                foreach (oriData originalData in observedData.originalDatas)
                {
                    if (originalData.type == "L" && originalData.value != 0)
                        angleobs.Add(new AngleObs(observedData.originalDatas[0].id, observedData.testSiteNum, originalData.id, originalData.value));
                    if (originalData.type == "S")
                        distanceobs.Add(new DistanceObs(observedData.testSiteNum, originalData.id, originalData.value));
                }
            }

            B = new DenseMatrix(angleobs.Count + distanceobs.Count, 2 * coordinate.points.Count);

            for (int i = 0; i < angleobs.Count; i++)
            {
                var A = coordinate.points.Find(item => item.id == angleobs[i].obsPoint);
                var P = coordinate.points.Find(item => item.id == angleobs[i].sigPoint);
                bool A_IsKnown = false, P_IsKnown = false;
                if (A == null)
                {
                    A_IsKnown = true;
                    A = knownData.pointDatas.Find(item => item.id == angleobs[i].obsPoint);
                }
                if (P == null)
                {
                    P_IsKnown = true;
                    P = knownData.pointDatas.Find(item => item.id == angleobs[i].sigPoint);
                }
                var alpha = BaseFunction.GetAzimuth(A.x, A.y, P.x, P.y);
                var S = Math.Sqrt((A.x - P.x) * (A.x - P.x) + (A.y - P.y) * (A.y - P.y));
                var a = 206265 * Math.Sin(alpha) / S / 100.0;
                var b = -206265 * Math.Cos(alpha) / S / 100.0;
                if (!A_IsKnown)
                {
                    B[i, 2 * coordinate.points.FindIndex(item => item == A)] = a;
                    B[i, 2 * coordinate.points.FindIndex(item => item == A) + 1] = b;
                }
                if (!P_IsKnown)
                {
                    B[i, 2 * coordinate.points.FindIndex(item => item == P)] = -a;
                    B[i, 2 * coordinate.points.FindIndex(item => item == P) + 1] = -b;
                }

            }
            for (int i = 0; i < distanceobs.Count; i++)
            {
                var A = coordinate.points.Find(item => item.id == distanceobs[i].observatingPoint);
                var P = coordinate.points.Find(item => item.id == distanceobs[i].sightingPoint);
                bool A_IsKnown = false, P_IsKnown = false;
                if (A == null)
                {
                    A_IsKnown = true;
                    A = knownData.pointDatas.Find(item => item.id == distanceobs[i].observatingPoint);
                }
                if (P == null)
                {
                    P_IsKnown = true;
                    P = knownData.pointDatas.Find(item => item.id == distanceobs[i].sightingPoint);
                }
                var alpha = BaseFunction.GetAzimuth(A.x, A.y, P.x, P.y);
                var a = Math.Cos(alpha);
                var b = Math.Sin(alpha);
                if (!A_IsKnown)
                {
                    B[i + angleobs.Count, 2 * coordinate.points.FindIndex(item => item == A)] = -a;
                    B[i + angleobs.Count, 2 * coordinate.points.FindIndex(item => item == A) + 1] = -b;
                }
                if (!P_IsKnown)
                {
                    B[i + angleobs.Count, 2 * coordinate.points.FindIndex(item => item == P)] = -a;
                    B[i + angleobs.Count, 2 * coordinate.points.FindIndex(item => item == P) + 1] = -b;
                }

            }


            P = new DenseMatrix(angleobs.Count + distanceobs.Count, angleobs.Count + distanceobs.Count);

            for (int i = 0; i < angleobs.Count; i++)
            {
                for (int j = 0; j < P.ColumnCount; j++)
                {
                    if (i == j)
                        P[i, j] = 1;
                    else
                        P[i, j] = 0;
                }
            }
            for (int i = 0; i < distanceobs.Count; i++)
            {
                for (int j = 0; j < P.ColumnCount; j++)
                {
                    if (i + angleobs.Count == j)
                    {
                        var xigmaB = knownData.Error;
                        var xigmaS = knownData.sError + Math.Pow(10, -6) * distanceobs[i].value;
                        P[i + angleobs.Count, j] = Math.Pow(xigmaB / xigmaS, 2);
                    }
                    else
                        P[i + angleobs.Count, j] = 0;
                }
            }

            l = new DenseVector(angleobs.Count + distanceobs.Count);

            for (int i = 0; i < angleobs.Count; i++)
            {
                var A = coordinate.points.Find(item => item.id == angleobs[i].basePoint);
                var B = coordinate.points.Find(item => item.id == angleobs[i].obsPoint);
                var C = coordinate.points.Find(item => item.id == angleobs[i].sigPoint);
                if (A == null)
                    A = knownData.pointDatas.Find(item => item.id == angleobs[i].basePoint);
                if (B == null)
                    B = knownData.pointDatas.Find(item => item.id == angleobs[i].obsPoint);
                if (C == null)
                    C = knownData.pointDatas.Find(item => item.id == angleobs[i].sigPoint);
                var alphaAB = BaseFunction.GetAzimuth(B.x, B.y, A.x, A.y);
                var alphaCB = BaseFunction.GetAzimuth(B.x, B.y, C.x, C.y);
                var deltaAlpha = alphaCB - alphaAB;
                if (deltaAlpha < 0)
                    deltaAlpha = 2 * Math.PI + deltaAlpha;
                var delta = BaseFunction.ToRad(angleobs[i].value) - deltaAlpha;
                l[i] = delta * 180 / Math.PI * 3600;
            }
            for (int i = 0; i < distanceobs.Count; i++)
            {
                var A = coordinate.points.Find(item => item.id == distanceobs[i].observatingPoint);
                var B = coordinate.points.Find(item => item.id == distanceobs[i].sightingPoint);
                if (A == null)
                    A = knownData.pointDatas.Find(item => item.id == distanceobs[i].observatingPoint);
                if (B == null)
                    B = knownData.pointDatas.Find(item => item.id == distanceobs[i].sightingPoint);
                var S = Math.Sqrt((A.x - B.x) * (A.x - B.x) + (A.y - B.y) * (A.y - B.y));
                l[i + angleobs.Count] = (distanceobs[i].value - S) * 1000;
            }
            N = B.Transpose() * P * B;
            W = B.Transpose() * P * l;
            X = N.Inverse() * W;
            V = B * X - l;
            var Qxx = V * P * V;
            var n = angleobs.Count + distanceobs.Count;
            var t = 2.0 * coordinate.points.Count;
            sigma0 = Math.Sqrt(Qxx / (n - t));
        }
    }
    internal class AngleObs
    {
        public string sigPoint, obsPoint, basePoint;
        public double value;
        public AngleObs(string referencePoint, string obsPoint, string sightingPoint, double value)
        {
            this.sigPoint = sightingPoint;
            this.obsPoint = obsPoint;
            this.basePoint = referencePoint;
            this.value = value;
        }

    }
    internal class DistanceObs
    {
        public string sightingPoint, observatingPoint;
        public double value;
        public DistanceObs(string observatingPoint, string sightingPoint, double value)
        {
            this.sightingPoint = sightingPoint;
            this.observatingPoint = observatingPoint;
            this.value = value;
        }
    }
}
